/* 
This file is part of FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry.

Developer: Chunran Zheng <zhengcr@connect.hku.hk>

For commercial use, please contact me at <zhengcr@connect.hku.hk> or
Prof. Fu Zhang at <fuzhang@hku.hk>.

This file is subject to the terms and conditions outlined in the 'LICENSE' file,
which is included as part of this source code package.
*/

// 防止头文件被重复包含
#ifndef COMMON_LIB_H
#define COMMON_LIB_H

// 包含自定义的 SO(3) 数学库
#include <utils/so3_math.h>
// 包含自定义的类型定义库
#include <utils/types.h>
// 包含自定义的颜色处理库
#include <utils/color.h>
// 包含 OpenCV 计算机视觉库
#include <opencv2/opencv.hpp>
// 包含 ROS 传感器消息中的 IMU 消息类型
#include <sensor_msgs/Imu.h>
// 包含 Sophus 库中的 SE(3) 李群表示
#include <sophus/se3.h>
// 包含 ROS 的 TF 变换广播器
#include <tf/transform_broadcaster.h>

// 使用标准命名空间
using namespace std;
// 使用 Eigen 线性代数库的命名空间
using namespace Eigen;
// 使用 Sophus 李群库的命名空间
using namespace Sophus;

// 打印当前文件和行号的宏定义
#define print_line std::cout << __FILE__ << ", " << __LINE__ << std::endl;
// 定义广东地区的重力加速度常量
#define G_m_s2 (9.81)   
// 定义状态向量的维度
#define DIM_STATE (19)  
// 定义初始协方差的值
#define INIT_COV (0.01)
// 定义大尺寸常量
#define SIZE_LARGE (500)
// 定义小尺寸常量
#define SIZE_SMALL (100)
// 从数组创建向量的宏定义
#define VEC_FROM_ARRAY(v) v[0], v[1], v[2]
// 从数组创建矩阵的宏定义
#define MAT_FROM_ARRAY(v) v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]
// 定义调试文件目录的宏定义
#define DEBUG_FILE_DIR(name) (string(string(ROOT_DIR) + "Log/" + name))

/**
 * @brief 定义激光雷达类型的枚举
 */
enum LID_TYPE {
  // Velodyne 公司的 AVIA 激光雷达
  AVIA = 1,      
  // Velodyne 公司的 VLP-16 激光雷达
  VELO16 = 2,    
  // Ouster 公司的 OS-64 激光雷达
  OUST64 = 3,    
  // Intel 公司的 RealSense L515 激光雷达
  L515 = 4,      
  // 速腾聚创公司的 RS-XT32 激光雷达
  XT32 = 5,      
  // 禾赛科技公司的 Pandar128 激光雷达
  PANDAR128 = 6  
};

/**
 * @brief 定义 SLAM 模式的枚举
 */
enum SLAM_MODE
{
  // 仅使用激光雷达里程计
  ONLY_LO = 0,
  // 仅使用激光雷达惯性里程计
  ONLY_LIO = 1,
  // 使用激光雷达惯性视觉里程计
  LIVO = 2
};

/**
 * @brief 定义 EKF 状态的枚举
 */
enum EKF_STATE
{
  // 等待状态
  WAIT = 0,
  // 视觉惯性里程计状态
  VIO = 1,
  // 激光雷达惯性里程计状态
  LIO = 2,
  // 激光雷达里程计状态
  LO = 3
};

// 存储视觉惯性里程计（VIO）和激光惯性里程计（LIO）相关的测量
struct MeasureGroup
{
  // 视觉惯性里程计的时间戳
  double vio_time;
  // 激光惯性里程计的时间戳
  double lio_time;
  // 存储 IMU 消息指针的双端队列
  deque<sensor_msgs::Imu::ConstPtr> imu;
  // 存储视觉图像的 OpenCV 矩阵
  cv::Mat img;

  /**
   * @brief 默认构造函数，初始化时间戳为 0
   */
  MeasureGroup()
  {
    vio_time = 0.0;
    lio_time = 0.0;
  };
};

/**
 * @brief 定义激光雷达测量组结构体
 */
struct LidarMeasureGroup
{
  // 激光雷达帧的开始时间
  double lidar_frame_beg_time;
  // 激光雷达帧的结束时间
  double lidar_frame_end_time;
  // 上一次激光惯性里程计更新的时间
  double last_lio_update_time;
  // 激光雷达点云数据指针
  PointCloudXYZI::Ptr lidar;
  // 当前处理的点云数据指针
  PointCloudXYZI::Ptr pcl_proc_cur;
  // 下一次处理的点云数据指针
  PointCloudXYZI::Ptr pcl_proc_next;
  // 存储测量组的双端队列
  deque<struct MeasureGroup> measures;
  // 激光惯性里程计的状态标志
  EKF_STATE lio_vio_flg;
  // 当前激光雷达扫描的索引
  int lidar_scan_index_now;

  /**
   * @brief 默认构造函数，初始化成员变量
   */
  LidarMeasureGroup()
  {
    lidar_frame_beg_time = -0.0;
    lidar_frame_end_time = 0.0;
    last_lio_update_time = -1.0;
    lio_vio_flg = WAIT;
    // 初始化激光雷达点云数据指针
    this->lidar.reset(new PointCloudXYZI());
    // 初始化当前处理的点云数据指针
    this->pcl_proc_cur.reset(new PointCloudXYZI());
    // 初始化下一次处理的点云数据指针
    this->pcl_proc_next.reset(new PointCloudXYZI());
    // 清空测量组队列
    this->measures.clear();
    lidar_scan_index_now = 0;
    last_lio_update_time = -1.0;
  };
};

/**
 * @brief 定义带协方差的点结构体
 */
typedef struct pointWithVar
{
  // 点在激光雷达本体坐标系下的坐标
  Eigen::Vector3d point_b;     
  // 点在 IMU 本体坐标系下的坐标
  Eigen::Vector3d point_i;     
  // 点在世界坐标系下的坐标
  Eigen::Vector3d point_w;     
  // 去除状态协方差后的协方差矩阵
  Eigen::Matrix3d var_nostate; 
  // 本体坐标系下的协方差矩阵
  Eigen::Matrix3d body_var;
  // 协方差矩阵
  Eigen::Matrix3d var;
  // 点的叉积矩阵
  Eigen::Matrix3d point_crossmat;
  // 点的法向量
  Eigen::Vector3d normal;

  /**
   * @brief 默认构造函数，初始化成员变量为零
   */
  pointWithVar()
  {
    var_nostate = Eigen::Matrix3d::Zero();
    var = Eigen::Matrix3d::Zero();
    body_var = Eigen::Matrix3d::Zero();
    point_crossmat = Eigen::Matrix3d::Zero();
    point_b = Eigen::Vector3d::Zero();
    point_i = Eigen::Vector3d::Zero();
    point_w = Eigen::Vector3d::Zero();
    normal = Eigen::Vector3d::Zero();
  };
} pointWithVar;

/**
 * @brief 定义状态组结构体
 */
struct StatesGroup
{
  /**
   * @brief 默认构造函数，初始化状态和协方差
   */
  StatesGroup()
  {
    // 初始化为单位矩阵
    this->rot_end = M3D::Identity();
    // 初始化为零向量
    this->pos_end = V3D::Zero();
    // 初始化为零向量
    this->vel_end = V3D::Zero();
    // 初始化为零向量
    this->bias_g = V3D::Zero();
    // 初始化为零向量
    this->bias_a = V3D::Zero();
    // 初始化为零向量
    this->gravity = V3D::Zero();
    // 初始化为 1.0
    this->inv_expo_time = 1.0;
    // 初始化为单位矩阵乘以初始协方差
    this->cov = MD(DIM_STATE, DIM_STATE)::Identity() * INIT_COV;
    // 特定位置的协方差设置为较小值
    this->cov(6, 6) = 0.00001;
    // 特定块的协方差设置为较小值
    this->cov.block<9, 9>(10, 10) = MD(9, 9)::Identity() * 0.00001;
  };

  /**
   * @brief 拷贝构造函数，复制另一个状态组的状态和协方差
   * @param b 要复制的状态组
   */
  StatesGroup(const StatesGroup &b)
  {
    this->rot_end = b.rot_end;
    this->pos_end = b.pos_end;
    this->vel_end = b.vel_end;
    this->bias_g = b.bias_g;
    this->bias_a = b.bias_a;
    this->gravity = b.gravity;
    this->inv_expo_time = b.inv_expo_time;
    this->cov = b.cov;
  };

  /**
   * @brief 赋值运算符重载，将另一个状态组的状态和协方差赋值给当前状态组
   * @param b 要赋值的状态组
   * @return 当前状态组的引用
   */
  StatesGroup &operator=(const StatesGroup &b)
  {
    this->rot_end = b.rot_end;
    this->pos_end = b.pos_end;
    this->vel_end = b.vel_end;
    this->bias_g = b.bias_g;
    this->bias_a = b.bias_a;
    this->gravity = b.gravity;
    this->inv_expo_time = b.inv_expo_time;
    this->cov = b.cov;
    return *this;
  };

  /**
   * @brief 加法运算符重载，将状态增量加到当前状态组上
   * @param state_add 状态增量向量
   * @return 新的状态组
   */
  StatesGroup operator+(const Matrix<double, DIM_STATE, 1> &state_add)
  {
    StatesGroup a;
    // 更新旋转矩阵
    a.rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0));
    // 更新位置
    a.pos_end = this->pos_end + state_add.block<3, 1>(3, 0);
    // 更新逆曝光时间
    a.inv_expo_time = this->inv_expo_time + state_add(6, 0);
    // 更新速度
    a.vel_end = this->vel_end + state_add.block<3, 1>(7, 0);
    // 更新陀螺仪偏置
    a.bias_g = this->bias_g + state_add.block<3, 1>(10, 0);
    // 更新加速度计偏置
    a.bias_a = this->bias_a + state_add.block<3, 1>(13, 0);
    // 更新重力加速度
    a.gravity = this->gravity + state_add.block<3, 1>(16, 0);

    // 协方差保持不变
    a.cov = this->cov;
    return a;
  };

  /**
   * @brief 复合加法运算符重载，将状态增量加到当前状态组上
   * @param state_add 状态增量向量
   * @return 当前状态组的引用
   */
  StatesGroup &operator+=(const Matrix<double, DIM_STATE, 1> &state_add)
  {
    // 更新旋转矩阵
    this->rot_end = this->rot_end * Exp(state_add(0, 0), state_add(1, 0), state_add(2, 0));
    // 更新位置
    this->pos_end += state_add.block<3, 1>(3, 0);
    // 更新逆曝光时间
    this->inv_expo_time += state_add(6, 0);
    // 更新速度
    this->vel_end += state_add.block<3, 1>(7, 0);
    // 更新陀螺仪偏置
    this->bias_g += state_add.block<3, 1>(10, 0);
    // 更新加速度计偏置
    this->bias_a += state_add.block<3, 1>(13, 0);
    // 更新重力加速度
    this->gravity += state_add.block<3, 1>(16, 0);
    return *this;
  };

  /**
   * @brief 减法运算符重载，计算当前状态组与另一个状态组的差值
   * @param b 另一个状态组
   * @return 状态差值向量
   */
  Matrix<double, DIM_STATE, 1> operator-(const StatesGroup &b)
  {
    Matrix<double, DIM_STATE, 1> a;
    // 计算旋转矩阵的差值
    M3D rotd(b.rot_end.transpose() * this->rot_end);
    // 计算旋转差值的对数映射
    a.block<3, 1>(0, 0) = Log(rotd);
    // 计算位置差值
    a.block<3, 1>(3, 0) = this->pos_end - b.pos_end;
    // 计算逆曝光时间差值
    a(6, 0) = this->inv_expo_time - b.inv_expo_time;
    // 计算速度差值
    a.block<3, 1>(7, 0) = this->vel_end - b.vel_end;
    // 计算陀螺仪偏置差值
    a.block<3, 1>(10, 0) = this->bias_g - b.bias_g;
    // 计算加速度计偏置差值
    a.block<3, 1>(13, 0) = this->bias_a - b.bias_a;
    // 计算重力加速度差值
    a.block<3, 1>(16, 0) = this->gravity - b.gravity;
    return a;
  };

  /**
   * @brief 重置姿态，将旋转、位置和速度重置为初始值
   */
  void resetpose()
  {
    // 重置为单位矩阵
    this->rot_end = M3D::Identity();
    // 重置为零向量
    this->pos_end = V3D::Zero();
    // 重置为零向量
    this->vel_end = V3D::Zero();
  }

  // 激光雷达最后一个点的估计姿态（旋转矩阵）
  M3D rot_end;                              
  // 激光雷达最后一个点的估计位置（世界坐标系）
  V3D pos_end;                              
  // 激光雷达最后一个点的估计速度（世界坐标系）
  V3D vel_end;                              
  // 估计的逆曝光时间（无尺度）
  double inv_expo_time;                     
  // 陀螺仪偏置
  V3D bias_g;                               
  // 加速度计偏置
  V3D bias_a;                               
  // 估计的重力加速度
  V3D gravity;                              
  // 状态协方差矩阵
  Matrix<double, DIM_STATE, DIM_STATE> cov; 
};

/**
 * @brief 设置 6D 位姿
 * @tparam T 数据类型
 * @param t 时间偏移
 * @param a 加速度
 * @param g 角速度
 * @param v 速度
 * @param p 位置
 * @param R 旋转矩阵
 * @return 6D 位姿对象
 */
template <typename T>
auto set_pose6d(const double t, const Matrix<T, 3, 1> &a, const Matrix<T, 3, 1> &g, const Matrix<T, 3, 1> &v, const Matrix<T, 3, 1> &p,
                const Matrix<T, 3, 3> &R)
{
  Pose6D rot_kp;
  // 设置时间偏移
  rot_kp.offset_time = t;
  for (int i = 0; i < 3; i++)
  {
    // 设置加速度
    rot_kp.acc[i] = a(i);
    // 设置角速度
    rot_kp.gyr[i] = g(i);
    // 设置速度
    rot_kp.vel[i] = v(i);
    // 设置位置
    rot_kp.pos[i] = p(i);
    for (int j = 0; j < 3; j++)
      // 设置旋转矩阵
      rot_kp.rot[i * 3 + j] = R(i, j);
  }
  // Map<M3D>(rot_kp.rot, 3,3) = R;
  return move(rot_kp);
}

#endif